/**
 *@note Copyright 2012-2022 CETHIK All Rights Reserved.
 *@brief 占位处理方法文件
 *@author tuyinan
 *@data 2022-04-xx
 */

#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>

#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>

namespace costmap_2d {

void calculateMinAndMaxDistances(
    const std::vector<costmap_2d::Point>& footprint, double& min_dist,
    double& max_dist) {
  min_dist = std::numeric_limits<double>::max();
  max_dist = 0.0;

  if (footprint.size() <= 2) {
    return;
  }

  for (unsigned int i = 0; i < footprint.size() - 1; ++i) {
    // check the distance from the robot center point to the first vertex
    double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
    double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
                                      footprint[i + 1].x, footprint[i + 1].y);
    min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
    max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
  }

  // we also need to do the last vertex and the first vertex
  double vertex_dist =
      distance(0.0, 0.0, footprint.back().x, footprint.back().y);
  double edge_dist =
      distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
                     footprint.front().x, footprint.front().y);
  min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
  max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}

costmap_2d::Point32 toPoint32(costmap_2d::Point pt) {
  costmap_2d::Point32 point32;
  point32.x = pt.x;
  point32.y = pt.y;
  point32.z = pt.z;
  return point32;
}

costmap_2d::Point toPoint(costmap_2d::Point32 pt) {
  costmap_2d::Point point;
  point.x = pt.x;
  point.y = pt.y;
  point.z = pt.z;
  return point;
}

// costmap_2d::Polygon toPolygon(std::vector<costmap_2d::Point> pts)
// {
//   costmap_2d::Polygon polygon;
//   for (int i = 0; i < pts.size(); i++){
//     polygon.points.push_back(toPoint32(pts[i]));
//   }
//   return polygon;
// }

std::vector<costmap_2d::Point> toPointVector(
    std::vector<std::vector<double> > polygon) {
  std::vector<costmap_2d::Point> pts;
  for (int i = 0; i < polygon.size(); i++) {
    costmap_2d::Point pt;
    pt.x = polygon.at(i).at(0);
    pt.y = polygon.at(i).at(1);
    pts.push_back(pt);
  }
  return pts;
}

void transformFootprint(double x, double y, double theta,
                        const std::vector<costmap_2d::Point>& footprint_spec,
                        std::vector<costmap_2d::Point>& oriented_footprint) {
  // build the oriented footprint at a given location
  oriented_footprint.clear();
  double cos_th = cos(theta);
  double sin_th = sin(theta);
  for (unsigned int i = 0; i < footprint_spec.size(); ++i) {
    costmap_2d::Point new_pt;
    new_pt.x =
        x + (footprint_spec[i].x * cos_th - footprint_spec[i].y * sin_th);
    new_pt.y =
        y + (footprint_spec[i].x * sin_th + footprint_spec[i].y * cos_th);
    oriented_footprint.push_back(new_pt);
  }
}

void padFootprint(std::vector<costmap_2d::Point>& footprint, double padding) {
  // pad footprint in place
  for (unsigned int i = 0; i < footprint.size(); i++) {
    costmap_2d::Point& pt = footprint[i];
    pt.x += sign0(pt.x) * padding;
    pt.y += sign0(pt.y) * padding;
  }
}

std::vector<costmap_2d::Point> makeFootprintFromRadius(double radius) {
  std::vector<costmap_2d::Point> points;

  // Loop over 16 angles around a circle making a point each time
  int N = 16;
  costmap_2d::Point pt;
  for (int i = 0; i < N; ++i) {
    double angle = i * 2 * M_PI / N;
    pt.x = cos(angle) * radius;
    pt.y = sin(angle) * radius;

    points.push_back(pt);
  }

  return points;
}
}  // namespace costmap_2d